#include "drop_cutter.hpp"
#include "boxtests.hpp"
#include <iostream>
#include <boost/numeric/ublas/io.hpp>
using namespace std;
 
  const double tolerance=0.00001;

       
  bool intersect_triangle(const triang &tr, double xc, double yc, double r,double *z)
{

  vector<double> z_vals;

  // #1 with points
  for(int i=0; i<3; i++)
    if((tr[i].x-xc)*(tr[i].x-xc)+(tr[i].y-yc)*(tr[i].y-yc) <= r*r)
      {
	//	cout<<"->"<<tr[i].z<<endl;
	 
      z_vals.push_back(tr[i].z);
      }
  if(z_vals.size()==3)
    return t_max(z_vals,z);
  
  // #2 with lines
  intersect_line(tr[0],tr[1],xc,yc,r,z_vals);
  intersect_line(tr[1],tr[2],xc,yc,r,z_vals);
  intersect_line(tr[2],tr[0],xc,yc,r,z_vals);

  // #3 with face

  double
    x1=tr[0].x, y1=tr[0].y, z1=tr[0].z,
    x2=tr[1].x, y2=tr[1].y, z2=tr[1].z,
    x3=tr[2].x, y3=tr[2].y, z3=tr[2].z;

  if(z1==z2 && z2==z3)
    if(in_tri(tr[0],tr[1],tr[2],xc,yc))
      {
	z_vals.push_back(z1);
	return t_max(z_vals,z);
      }
      
  
  double D=
    x1*y2*z3+x3*z2*y1+z1*x2*y3-
    z1*y2*x3-z2*y3*x1-x2*y1*z3;
  if(abs(D)<tolerance)///???????!!!!!!!!!!
    return t_max(z_vals,z);

  double
    vx1=x2-x1,
    vy1=y2-y1,
    vz1=z2-z1,

    vx2=x3-x2,
    vy2=y3-y2,
    vz2=z3-z2;
  

      
  double
    a=vy1*vz2-vy2*vz1,
    b=vz1*vx2-vx1*vz2,
    c=vx1*vy2-vx2*vy1,
    d=-a*x1-b*y1-c*z1;

  double t1=atan(b/a);
  double t2=PI+t1;
  
 
   x1=xc+r*cos(t1),
   y1=yc+r*sin(t1),
   x2=xc+r*cos(t2),
   y2=yc+r*sin(t2);

   if(in_tri(tr[0],tr[1],tr[2],x1,y1))
     z_vals.push_back((a*x1+b*y1+d)/-c);
   
   if(in_tri(tr[0],tr[1],tr[2],x2,y2))
     z_vals.push_back((a*x2+b*y2+d)/-c);

 return t_max(z_vals,z);
 

}

 bool in_tri(point p1, point p2, point p3,
	    double x, double y)
{
  return
    iz_right(p1,p2,x,y)&&
    iz_right(p2,p3,x,y)&&
    iz_right(p3,p1,x,y);
  
}


void intersect_line(point p1, point p2, double xc, double yc, double r,vector<double> &z_vals){
  if(abs(p1.x-p2.x)<tolerance &&
     abs(p1.x-p2.x)<tolerance &&
     (p1.x-xc)*(p1.x-xc)+(p1.y-yc)*(p1.y-yc) <= r*r )
    {
    if(p1.z>p2.z)
      z_vals.push_back(p1.z);
    else
      z_vals.push_back(p2.z);
     
    return;
  }

  if(abs(p1.x-p2.x)<tolerance)
    {
      //  cerr<<"*******GOTTAAA!!!"<<endl;
      
      if(p1.z>p2.z){
	point p;
	p=p1;
	p1=p2;
	p2=p;
      }
      
      double y1=yc+sqrt(r*r-(p1.x-xc)*(p1.x-xc));
      double y2=yc-sqrt(r*r-(p1.x-xc)*(p1.x-xc));

      double kz=(p2.z-p1.z)/(p2.y-p1.y),
	bz=p1.z-kz*p1.y;

      double z1=kz*y1+bz;
      double z2=kz*y2+bz;

      double ymin=min(p1.y,p2.y);
      double ymax=max(p1.y,p2.y);
      
      
      if(z1>=p1.z && z1<=p2.z && y1>=ymin && y1<=ymax)
	z_vals.push_back(z1);
      if(z2>=p1.z && z2<=p2.z && y1>=ymin && y1<=ymax)
	z_vals.push_back(z2); 
    }
  
      
      
  
  if(p1.x> p2.x){
    point p;
    p=p1;
    p1=p2;
    p2=p;
  }

  double
    k=(p2.y-p1.y)/(p2.x-p1.x),
    b=p1.y-k*p1.x,
    kz=(p2.z-p1.z)/(p2.x-p1.x),
    bz=p1.z-kz*p1.x;

  double
    A=(1+k*k),
    B=2*(k*b-xc-k*yc),
    C=(xc*xc+b*b-2*b*yc+yc*yc-r*r);
  double x1,x2;
  int c=solve_sq(A,B,C,x1,x2);
  
  if(c>0)
    if(x1>=p1.x && x1<=p2.x)
      z_vals.push_back(kz*x1+bz);
  if(c>1)
    if(x2>=p1.x && x2<=p2.x)
      z_vals.push_back(kz*x2+bz);


}
  
  

 int solve_sq(double a, double b, double c, double &x1, double &x2){
  double D=b*b-4*a*c;
  if(D<0) 
    return 0;
  if(D==0){
    x1=-b/2/a;
    return 1;
  }
  else{
    x1=(-b+sqrt(D))/2/a;
    x2=(-b-sqrt(D))/2/a;
    return 2;
  }
}


 bool iz_right(point p1, point p2, double x, double y)
{
  double
    x1=p2.x-p1.x,
    x2=p2.x-x,
    y1=p2.y-p1.y,
    y2=p2.y-y;

  return x1*y2-x2*y1<0;
}

double max(const vector<double> &v)
{ if(v.size()==0) return 0;
  
  vector<double>::const_iterator p= v.begin();

  double m=*p;
 
  
  for(;p!=v.end();p++)
    if(*p>m)
      m=*p;

  return m;
  
}


bool t_max(const vector<double> &v,double *r)
{
  //cout<<"entered\n";
  
  if(v.size()==0){/*cout<<":(\n";*/ return false;
  }
  
  
  vector<double>::const_iterator p= v.begin();

  double m=*p;
  //cout<<m<<endl;
  
  
  for(;p!=v.end();p++)
    if(*p>m)
      {
	
      m=*p;
      //  cout<<m<<endl;
      }
  

  *r=m;

  // cout<<"-->"<<*r<<endl;
  
  return true;
  
  
}

// bool cross_test2d(point a1, point a2, point b1, point b2)
// {
//   //special cases;
//   if(a1.x==a2.x && b1.x==b2.x )
//     if(a1.x==b1.x)
//       {
// 	if(a1.y>a2.y)
// 	  swap(a1,a2);
// 	if(b1.y>b2.y)
// 	  swap(b1,b2);  

// 	return
// 	  (a1.y>=b1.y && a1.y<=b2.y) ||
// 	  (a2.y>=b1.y && a2.y<=b2.y) ||
// 	  (b1.y>=a1.y && b1.y<=a2.y) ||
// 	  (b2.y>=a1.y && b2.y<=a2.y);
	
//       }
//     else
//       return false;
  
//   //general case; y1=k1*x+c1 ; y2=k2*x+c2 
    
//   if(a1.x>a2.x)
//     swap(a1,a2);

//   if(b1.x>b2.x)
//     swap(b1,b2);

//   double
//     k1=(a2.y-a1.y)/(a2.x-a1.x),
//     �1=a1.y-k1*a1.x;
//   double
//     k2=(b2.y-b1.y)/(b2.x-b1.x),
//     �2=b1.y-k2*b1.x;

//   double x=(b2-b1)/(k1-k2);

//   return x>=a1.x && x<=a2.x;
// }

  
bool cross_test2d(point a1, point a2, point b1, point b2)
{
  double
    A1=a1.y-a2.y,
    B1=-(a1.x-a2.x),
    C1=-A1*a1.x-B1*a1.y;

  double
    A2=b1.y-b2.y,
    B2=-(b1.x-b2.x),
    C2=-A2*b1.x-B2*b1.y;

  return
    (A1*b1.x+B1*b1.y+C1)*(A1*b2.x+B1*b2.y+C1)<=0 &&
    (A2*a1.x+B2*a1.y+C2)*(A2*a2.x+B2*a2.y+C2)<=0;
}
 
         
